#ifndef ROBOT_H
#define ROBOT_H
#include <gsl/gsl_matrix.h>

/* Constantes */
#define L1 0.5
#define L2 0.5

/* Límites de ángulos */
#define THETA_MAX 170.0*M_PI/180.0
#define THETA_MIN -170.0*M_PI/180.0

/* Tipo booleano */
#define FALSE 0
#define TRUE !FALSE
typedef int Bool;

/* DOCUMENTACIÓN MATRICES GSL: 
 * http://www.gnu.org/software/gsl/manual/html_node/Matrices.html */

/* NOTA1: Las funciones están implementadas con pasaja por punteros para evitar
 * la copia de la estructura frame en cada llamada y porque las funciones de
 * la GSL están implementadas de ese modo también
 */

/* NOTA2: Para ocultar aquellas funciones "privadas" que deseen ocultarse efec-
 * tivamente, cortar los prototipos y pegarlos en robot.c
 */

/* NOTA3: Todas las funciones que devuelven punteros a frame suponen que el lla-
 * mador no ha asignado memoria para los frames, por lo que lo asignan ellas.
 * Si el llamador ha asignado memoria previamente se producirán fugas de memoria
 * El llamador debe encargarse de liberar la memoria asignada por estas funcio-
 * nes una vez que ya no necesite los resultados de las mismas
 */

/* Sinónimos de tipos */
typedef double vec3[3];
typedef gsl_matrix frame;

/* Prototipos funciones */
frame* UTOI(vec3 uform);
/* Retorna la representacion de un sistema de referencia de su forma
 * interna (frame) a partir de su forma de usuario (vec3 uform).
 * INPUT: uform = (x,y,theta) -- theta viene dado en grados
 * OUTPUT: matriz 3x3 con el siguiente contenido:
 *         (cos(theta)  -sen(theta) x
 *          sen(theta)  cos(theta)  y
 *          0           0           1)
 */

void ITOU(frame *iform, vec3 uform);
/* Retorna en uform la forma del usuario de representar un sistema de
 * referencia (vec3) a partir de la forma interna de representacion frame
 * (iform).
 * INPUT: iform en forma matricial
 * OUTPUT: uform vector de 3 componentes
 */

frame* TMULT(frame *brela, frame *crelb);
/* INPUT: brela (B relativo a A), crelb (C relativo a B)
 * OUTPUT: devuelve un puntero a la matriz que resulta de multiplicar
 *         brela*crelb
 */

frame* TINVERT(frame *brela);
/* INPUT: brela (B relativo a A)
 * OUTPUT: devuelve un puntero a una matriz que es la inversa de brela,
 * o sea arelb (A relativo a B)
 */

void mul_matrix(frame *m1, frame *m2, frame *res);
/* INPUT: matrices m1 y m2 de 3*3
 * OUTPUT: Devuelve en res el producto matricial m1.m2
 * Esta función es privada y no debe usarse, en su lugar usar TMULT
 */

frame* KIN(vec3 theta);
/* Implementa la cinematica directa del robot: a partir de las variables
 * de las articulaciones (los angulos contenidos en theta) retorna un puntero 
 * a la representacion interna de wrelb (Wrist relativo a Base)
 * INPUT: theta es un vector que contiente tres ángulos dados en grados
 * OUTPUT: puntero al frame wrelb
 */

frame *WHERE(vec3 theta, frame *trelw, frame *srelb);
/* Retorna un puntero al frame trels (Tool relativo a Station) calculado
 * a partir de los angulos de articulaciones (theta) y de los frames 
 * trelw (Tool relativo a Wrist) y srelb (Station relativo a Base)
 * INPUT: theta, trelw y srelb
 * OUTPUT: puntero al frame trels
 */

void print_frame(frame *fr, FILE *f);
/* Imprime  en forma matricial el frame fr en el archivo f. Para imprimir por
 * consola el segundo argumento debe ser stdout
 */ 

void print_vec3(vec3 v, FILE *f);
/* Imprime  vec3 en el archivo f. Para imprimir por consola el segundo argumento 
 * debe ser stdout
 */ 

Bool INVKIN(frame *wrelb, vec3 current, vec3 near, vec3 far);
/* Implementa la cinematica inversa: a partir de wrelb (Wrist relativo 
 * a Base) calcula las dos soluciones near y far para los angulos de las
 * articulaciones que conducen a esa wrelb. current son los valores actuales
 * de los angulos de articulacion. Clasifica a near y far de acuerdo a la
 * cercania de la segunda articulacion (donde se mide theta_2) en la posicion
 * actual con la de cada solucion. 
 * Retorna TRUE si hay solución, FALSE caso contrario. En el caso que una de
 * las dos soluciones no sea permitida porque al menos una de las articulaciones
 * requiere un angulo que no esta en el rango permitido (THETA_MIN, THETA_MAX)
 * retorna near=far=(unica solucion permitida).
 * INPUTS: wrelb y current
 * OUTPUTS: far y near
 */

Bool SOLVE(frame *trels, frame *trelw, frame *srelb, vec3 current, vec3 near, vec3 far);
/* INPUTS: todos los argumentos */

#endif



